/**
 ******************************************************************************
 * @文件    sbus_parser.c
 * @作者    SBUS解析器
 * @版本    V1.0
 * @日期    2025-11-09
 * @概要    SBUS协议解析器实现文件
 ******************************************************************************
 */

#include "sbus_ht10a.h"

#include <stdio.h>
#include <string.h>

#include "bsp_uni_remote.h"
#include "main.h"  //hal_gettick
#include "stdint.h"

// SBUS帧长度
#define SBUS_FRAME_LENGTH 25
#define FALSE 0
#define TRUE 1

/**
 * @brief  初始化SBUS解析器
 * @param  hsbus: SBUS句柄
 */
void SBUS_Init(SBUS_HandleTypeDef* parser) {
    if (parser == NULL)
        return;

    memset(parser, 0, sizeof(SBUS_HandleTypeDef));
    parser->connected = FALSE;
    parser->last_update = 0;
}

/**
 * @brief  更新SBUS数据
 * @param  hsbus: SBUS句柄
 * @param  data: 原始数据指针
 * @param  len: 数据长度 ==25
 * @retval 解析是否成功
 */
uint8_t SBUS_Update(SBUS_HandleTypeDef* hsbus, uint8_t* data, uint32_t len) {
    if (hsbus == NULL || data == NULL || len != SBUS_FRAME_LENGTH) {
        return FALSE;
    }

    // 检查帧头和帧尾 如果帧头不是0x0F 或者 帧尾不是0x00，就返回 FALSE
    if (data[0] != 0x0F || data[SBUS_FRAME_LENGTH - 1] != 0x00) {
        return FALSE;
    }

    // 拷贝原始数据
    memcpy(&hsbus->raw_data, data, sizeof(SBUS_RawData));

    // 解析通道数据
    SBUS_ParseChannels(hsbus);

    hsbus->connected = TRUE;
    hsbus->last_update = HAL_GetTick();

    return TRUE;
}

/**
 * @brief  解析通道数据
 * @param  hsbus: SBUS句柄
 */

void SBUS_ParseChannels(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL)
        return;

    uint8_t* data = hsbus->raw_data.data;

    // 解析16个通道的11位数据
    hsbus->channels_raw[0] = ((data[0] | data[1] << 8) & 0x07FF);
    hsbus->channels_raw[1] = ((data[1] >> 3 | data[2] << 5) & 0x07FF);
    hsbus->channels_raw[2] = ((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF);
    hsbus->channels_raw[3] = ((data[4] >> 1 | data[5] << 7) & 0x07FF);
    hsbus->channels_raw[4] = ((data[5] >> 4 | data[6] << 4) & 0x07FF);
    hsbus->channels_raw[5] = ((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF);
    hsbus->channels_raw[6] = ((data[8] >> 2 | data[9] << 6) & 0x07FF);
    hsbus->channels_raw[7] = ((data[9] >> 5 | data[10] << 3) & 0x07FF);
    hsbus->channels_raw[8] = ((data[11] | data[12] << 8) & 0x07FF);
    hsbus->channels_raw[9] = ((data[12] >> 3 | data[13] << 5) & 0x07FF);
    hsbus->channels_raw[10] = ((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF);
    hsbus->channels_raw[11] = ((data[15] >> 1 | data[16] << 7) & 0x07FF);
    hsbus->channels_raw[12] = ((data[16] >> 4 | data[17] << 4) & 0x07FF);
    hsbus->channels_raw[13] = ((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF);
    hsbus->channels_raw[14] = ((data[19] >> 2 | data[20] << 6) & 0x07FF);
    hsbus->channels_raw[15] = ((data[20] >> 5 | data[21] << 3) & 0x07FF);

    /**
         通道值对应遥感映射关系
         channels_raw[0] 通道1: 右摇杆X 192~992~1792
         channels_raw[1] 通道2: 右摇杆Y 192~992~1792
         channels_raw[2] 通道3: 左摇杆Y 192~992~1792
         channels_raw[3] 通道4: 左摇杆X 192~992~1792
         channels_raw[4] 通道5: SWA-5  192/992/1792   三档自弹
         channels_raw[5] 通道6: SWB-6  192/1792       两档
         channels_raw[6] 通道7: SWC-7  192/1792       两档
         channels_raw[7] 通道8: SWD-8  192/992/1792   三档
         channels_raw[8] 通道9: VRA    192~1792       旋钮
         channels_raw[9] 通道10 VRB    192~1792       旋钮
         channels_raw[x] 通道11~16                    保留
    **/

    // 解析标志位
    uint8_t flags = hsbus->raw_data.flags;
    hsbus->ch17 = (flags & SBUS_FLAG_CH17) != 0;
    hsbus->ch18 = (flags & SBUS_FLAG_CH18) != 0;
    hsbus->frame_lost = (flags & SBUS_FLAG_LOST) != 0;
    hsbus->failsafe = (flags & SBUS_FLAG_FAILSAFE) != 0;
}

/**
 * @brief  解析通道数据
 * @param  hsbus: SBUS句柄
 */
void SBUS_To_UniRemote(UniRemote_HandleTypeDef* hrc, SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL)
        return;

    // 传入信号是否丢失信息
    hrc->frame_lost = hsbus->frame_lost;
    // 传入信号是否连接信息
    hrc->frame_connected = hsbus->connected;

    if (hrc->frame_connected && !hrc->frame_lost) {
        // 转换成rbus 数据
        hrc->right_x = SBUS_To_RBUS_Range(hsbus->channels_raw[0]);  // 通道1: 右摇杆X
        hrc->right_y = SBUS_To_RBUS_Range(hsbus->channels_raw[1]);  // 通道2: 右摇杆Y
        hrc->left_y = SBUS_To_RBUS_Range(hsbus->channels_raw[2]);   // 通道3: 左摇杆Y
        hrc->left_x = SBUS_To_RBUS_Range(hsbus->channels_raw[3]);   // 通道4: 左摇杆X

        // 映射开关状态 (根据通道值判断三档开关)
        // hrc->switch_mode = SBUS_Map_RBUS_Switch(hsbus->channels_raw[4]); // 使用通道5作为左1开关  保留
        hrc->switch_rate = SBUS_Map_RBUS_Switch(hsbus->channels_raw[5]);   // 使用通道8作为右2开关  0 电控 1 pid 模式
        hrc->switch_mode = SBUS_Map_RBUS_Switch(hsbus->channels_raw[6]);   // 使用通道7作为左2开关  避障
        hrc->switch_power = SBUS_Map_RBUS_Switch(hsbus->channels_raw[7]);  // 使用通道6作为右1开关  0 关机 1 麦轮模式 2：普通轮模式
    } else {
        // 丢失信号时，将所有数据置零
        hrc->right_x = 0;
        hrc->right_y = 0;
        hrc->left_y = 0;
        hrc->left_x = 0;
        hrc->switch_mode = RBUS_SWITCH_OFF;
        hrc->switch_power = RBUS_SWITCH_OFF;
        hrc->switch_rate = RBUS_SWITCH_OFF;
    }
}

/**
 * @brief  将SBUS原始值转换RBUS标准范围
 * @param  sbus_value: SBUS原始值 (192-1792)
 * @retval RBUS标准范围值 (-100 ~ 100)
 */
int16_t SBUS_To_RBUS_Range(uint16_t sbus_value) {
    int32_t result;

    // 限制输入范围不越界
    sbus_value = (sbus_value < SBUS_CH_MIN) ? SBUS_CH_MIN : (sbus_value > SBUS_CH_MAX) ? SBUS_CH_MAX
                                                                                       : sbus_value;

    // 线性映射计算
    // 公式: output = (input - mid) * range_ratio
    if (sbus_value > SBUS_CH_MID) {
        // 正值范围计算
        result = (int32_t)(sbus_value - SBUS_CH_MID) * RBUS_CH_MAX;
        result /= (SBUS_CH_MAX - SBUS_CH_MID);
    } else if (sbus_value < SBUS_CH_MID) {
        // 负值范围计算
        result = (int32_t)(sbus_value - SBUS_CH_MID) * RBUS_CH_MAX;
        result /= (SBUS_CH_MID - SBUS_CH_MIN);
    } else {
        // 中位值
        result = RBUS_CH_MID;
    }

    return (int16_t)result;
}

/**
 * @brief  映射开关状态 192:0 992:1 1992:2
 * @param  channel_value: 通道原始值
 * @retval 开关状态 (SWITCH_UP/MID/DOWN)
 */
uint8_t SBUS_Map_RBUS_Switch(uint16_t channel_value) {
    // 根据通道值映射三档开关 0是转换后的中间位置
    if (channel_value < SBUS_CH_MID)
        return RBUS_SWITCH_DOWN;  // 下档
    else if (channel_value > SBUS_CH_MID)
        return RBUS_SWITCH_UP;  // 上档
    else
        return RBUS_SWITCH_MID;  // 中档
}

/**
 * @brief  检查连接超时
 * @param  hsbus: SBUS句柄
 * @retval 是否超时
 */
uint8_t SBUS_CheckTimeout(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL) {
        return 1;
    }
    uint32_t current_tick = HAL_GetTick();
    if (current_tick - hsbus->last_update > SBUS_TIMEOUT_MS) {
        hsbus->connected = FALSE;
        return 1;
    }

    return 0;
}

/**
 * @brief  打印通道数据（详细版）
 * @param  hsbus: SBUS句柄
 */
void SBUS_PrintChannels(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL || !hsbus->connected) {
        printf("SBUS: No data or disconnected\n");
        return;
    }

    printf("=== SBUS All Channels ===\n");
    // 打印原始值
    printf("Raw Values (0-2047):\n");
    for (int i = 0; i < 16; i++) {
        printf("CH%2d: %4d", i + 1, hsbus->channels_raw[i]);
        if ((i + 1) % 4 == 0)
            printf("\n");
        else
            printf(" | ");
    }

    printf("Digital: CH17=%d, CH18=%d\n", hsbus->ch17, hsbus->ch18);
    printf("Status: Lost=%d, Failsafe=%d\n", hsbus->frame_lost, hsbus->failsafe);
    printf("Connection: %s, Last Update: %lu ms\n",
           hsbus->connected ? "OK" : "Lost", hsbus->last_update);
}

/**
 * @brief  获取通道原始值
 * @param  hsbus: SBUS句柄
 * @param  channel: 通道号 (0-15)
 * @retval 通道原始值 (0-2047)，如果错误返回0
 */
uint16_t SBUS_GetChannelRaw(SBUS_HandleTypeDef* hsbus, uint8_t channel) {
    if (hsbus == NULL || channel >= 16 || !hsbus->connected) {
        return 0;
    }

    return hsbus->channels_raw[channel];
}

/**
 * @brief  获取连接状态
 * @param  hsbus: SBUS句柄
 * @retval 连接状态
 */
uint8_t SBUS_IsConnected(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL)
        return FALSE;

    // 检查超时
    SBUS_CheckTimeout(hsbus);

    return hsbus->connected;
}

/**
 * @brief  获取帧丢失状态
 * @param  hsbus: SBUS句柄
 * @retval 帧丢失状态
 */
uint8_t SBUS_IsFrameLost(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL)
        return TRUE;

    return hsbus->frame_lost;
}

/**
 * @brief  获取故障安全状态
 * @param  hsbus: SBUS句柄
 * @retval 故障安全状态
 */
uint8_t SBUS_IsFailsafe(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL)
        return TRUE;

    return hsbus->failsafe;
}

/**
 * @brief  获取数字通道状态
 * @param  hsbus: SBUS句柄
 * @param  channel: 数字通道号 (17或18)
 * @retval 数字通道状态
 */
uint8_t SBUS_GetDigitalChannel(SBUS_HandleTypeDef* hsbus, uint8_t channel) {
    if (hsbus == NULL)
        return FALSE;

    if (channel == 17) {
        return hsbus->ch17;
    } else if (channel == 18) {
        return hsbus->ch18;
    }

    return FALSE;
}

/**
 * @brief  重置解析器状态
 * @param  hsbus: SBUS句柄
 */
void SBUS_Reset(SBUS_HandleTypeDef* hsbus) {
    if (hsbus == NULL)
        return;

    SBUS_Init(hsbus);
}

/******************* (C) COPYRIGHT 2024 SBUS Parser **************************/